#ifndef TOF_HPP
#define TOF_HPP

#include <opencv2/opencv.hpp>
#include "ply.hpp"
#include "ransacplane.hpp"

static void ComputeRay(cv::Mat& ray, cv::Size size, const cv::Mat& K, const cv::Mat& D)
{
    cv::Mat xy(size, CV_64FC2);

    for (int i = 0; i < xy.rows; i++)
    {
        for (int j = 0; j < xy.cols; j++)
        {
            xy.at<cv::Vec2d>(i, j) = cv::Vec2d(j, i);
        }
    }

    xy = xy.reshape(2, size.area());

    cv::undistortPoints(xy, ray, K, D);

    ray=ray.reshape(2, size.height);
}

static void depth2xyz(cv::Mat& xyz, const cv::Mat& depth, const cv::Mat& K, const cv::Mat& D)
{
    cv::Mat ray;
    ComputeRay(ray, depth.size(), K, D);

    xyz.create(ray.size(), CV_64FC3);

    for (int i = 0; i < ray.rows; i++)
    {
        for (int j = 0; j < ray.cols; j++)
        {
            cv::Vec2d xy1 = ray.at<cv::Vec2d>(i, j);
            unsigned int z = depth.at<unsigned short>(i, j) & 0x1fff;
            xyz.at<cv::Vec3d>(i, j) = cv::Vec3d(xy1[0], xy1[1], 1)*(double)z;
        }
    }
}


static void saveply(const cv::Mat& xyz, std::string plyPath)
{
    PLY aa;

    for (int i = 0; i < xyz.rows; i++)
    {
        for (int j = 0; j < xyz.cols; j++)
        {
            cv::Vec3d xyz1 = xyz.at<cv::Vec3d>(i, j);
            if (xyz1[2] > 0)
            {
                aa.xyz.push_back(xyz1[0]);
                aa.xyz.push_back(xyz1[1]);
                aa.xyz.push_back(xyz1[2]);
            }
        }
    }

    aa.rgb.resize(aa.xyz.size());
    aa.nxyz.resize(aa.xyz.size());

    aa.Save(plyPath);
}


static void depth2ply(std::string depthPath, std::string plyPath)
{
    cv::Mat dd=cv::imread(depthPath, cv::IMREAD_UNCHANGED);
    std::cout<<dd.elemSize()<<"\n";

    cv::Mat K = (cv::Mat_<double>(3, 3) << 217.869247, 0, 110.485161, 0, 217.869247, 84.037331, 0, 0, 1);
    cv::Mat D = (cv::Mat_<double>(5, 1) << -0.136084, -1.520149, -0.000889, 0.000396, 3.170387);

    cv::Mat xyz;

    depth2xyz(xyz, dd, K, D);

    saveply(xyz, plyPath);
}

static void DecodeBin(char *data, unsigned int length, std::string name)
{

    if(length==2064*1548 * 2)
    {
        cv::Mat m(1548*3/2, 2064, CV_8UC1, data);

        cv::Mat bgr;
        cv::cvtColor(m, bgr, cv::COLOR_YUV2BGR_NV21);

        cv::imwrite(name+".png", bgr);

        return;
    }

    if(length==172 * 224  * 2*2)
    {
        cv::Mat d(172, 224, CV_16UC1,  data);
        for(size_t i=0;i<d.total();i++)
            d.at<unsigned short>(i) &= 0x1fff;
        cv::imwrite(name+"d.png", d);

        cv::Mat r(172, 224, CV_8UC1,  data+172*224*2);
        cv::imwrite(name+"r.png", r);

        //        depth2ply(name+"d.png", name+".ply");

        return;
    }
}

static void DecodeBin(std::string path)
{
    std::stringstream ss;

    if(!FileBase::LoadBinaryStream(path.c_str(), ss))
        return;

    std::string str = ss.str();

    std::string name, ext;

    StrBase::SpiltExtention(name, ext, path);

    DecodeBin((char*)str.data(), str.size(), name);
}



static void PolygonMask(cv::Mat& mask, cv::Size size, const std::vector<cv::Point>& vertex)
{
    mask.create(size, CV_8UC1);
    mask.setTo(0);
    std::vector< std::vector<cv::Point> > pts(1, vertex);
    cv::fillPoly(mask, pts, cv::Scalar(255));
}


static float CalDistance(const cv::Mat& depth, const cv::Mat& K, const cv::Mat& D, const cv::Mat& mask)
{
    cv::Mat ray;
    ComputeRay(ray, depth.size(), K, D);

    std::vector<float> ds;
    for(size_t i=0;i<ray.total();i++)
    {
        cv::Vec2d &xy=ray.at<cv::Vec2d>(i);
        unsigned short z = depth.at<unsigned short>(i) & 0x1fff;
        if(mask.at<uchar>(i)>0 && z>0)
        {
            double a1=z*cv::norm(cv::Vec3d(xy[0],xy[1],1));
            ds.push_back(a1);
        }
    }

    if(ds.empty())
        return -1;

    size_t nMiddle=ds.size()/2;

    std::nth_element(ds.begin(), ds.begin()+nMiddle, ds.end());

    return ds[nMiddle];
}

static cv::Vec4f CalPlane(const cv::Mat& depth, const cv::Mat& K, const cv::Mat& D, const cv::Mat& mask)
{
    cv::Mat xyz;

    depth2xyz(xyz, depth, K, D);

    cv::Mat masknot;
    cv::bitwise_not(mask, masknot);

    xyz.setTo(cv::Vec3d(0,0,0), masknot);

    std::vector<cv::Vec3f> ps;
    for(size_t i=0;i<xyz.total();i++)
    {
        cv::Vec3d &xyz1=xyz.at<cv::Vec3d>(i);
        if(xyz1[2]>0)
        {
            ps.push_back(cv::Vec3f(xyz1[0],xyz1[1],xyz1[2]));
        }
    }

    RansacPlane rpf;
    rpf.Setup(ps, 0.99, 3);
    size_t acm=0;
    rpf.Fit(acm, 16, 10000);

    std::cout<<rpf.EvaluateErrorAll()<<"\n";


    std::cout<<rpf.model<<"\n"<<((double)acm)/ps.size()<<"\n";

    if(rpf.model[3]<0) rpf.model=-rpf.model;

    return rpf.model;

    cv::Vec4f bp=RansacPlane::BestPlane(ps);
    std::cout<<bp<<"\n";

    rpf.model=bp;

    std::cout<<rpf.EvaluateErrorAll()<<"\n";

    if(bp[3]<0) bp=-bp;

    return bp;
}


static void PlaneProjection(cv::Vec4f plane, cv::Matx33f& R, cv::Vec3f& T)
{
    cv::Vec3f zAxis(plane[0],plane[1],plane[2]);

    T=zAxis*(-plane[3]);

    cv::Vec3f xAxis=cv::Vec3f(0,1,0).cross(zAxis);
    cv::Vec3f yAxis=zAxis.cross(xAxis);

    R=cv::Matx33f(xAxis[0], xAxis[1], xAxis[2],
            yAxis[0], yAxis[1], yAxis[2],
            zAxis[0], zAxis[1], zAxis[2]);
}


static cv::Vec3f MeasureObject(const cv::Mat& depth, const cv::Mat& K, const cv::Mat& D, const cv::Mat& mask,
                               cv::Vec4f table, float heightCut, size_t pointNumberThreshold, float heightRatio=0.95)
{
    cv::Mat xyz;

    depth2xyz(xyz, depth, K, D);

    cv::Mat masknot;
    cv::bitwise_not(mask, masknot);

    xyz.setTo(cv::Vec3d(0,0,0), masknot);

    std::vector<cv::Vec3f> ps;
    for(size_t i=0;i<xyz.total();i++)
    {
        cv::Vec3d &xyz1=xyz.at<cv::Vec3d>(i);
        if(xyz1[2]>0)
        {
            ps.push_back(cv::Vec3f(xyz1[0],xyz1[1],xyz1[2]));
        }
    }

    cv::Matx33f R;
    cv::Vec3f T;

    PlaneProjection(table, R, T);

    std::vector<cv::Vec2f> xyCut;
    std::vector<float> zCut;

    for(auto &p:ps)
    {
        cv::Vec3f ppr=R*(p-T);
        if(ppr[2]>heightCut)
        {
            xyCut.push_back(cv::Vec2f(ppr[0],ppr[1]));
            zCut.push_back(ppr[2]);
        }
    }

    std::cout<<"found object of "<<xyCut.size()<<" points\n";


    if(xyCut.size()<pointNumberThreshold)
        return cv::Vec3f(0,0,0);

    cv::RotatedRect rr=cv::minAreaRect(xyCut);

    float objectH=0;
    if(heightRatio<1 && heightRatio>0)
    {
        std::sort(zCut.begin(),zCut.end());
        objectH=zCut[zCut.size()*heightRatio];
    }
    else
    {
        objectH=*std::max_element(zCut.begin(),zCut.end());
    }


    if(rr.size.height>rr.size.width)
        return cv::Vec3f(rr.size.height, rr.size.width, objectH);

    return cv::Vec3f(rr.size.width, rr.size.height, objectH);
}

template<typename T>
T median(std::vector<T> &n)
{
    std::sort(n.begin(), n.end());

    auto size = n.size();
    if ((size % 2) == 0) {
        return (n[size/2] + n[(size/2) - 1])/2;
    }

    return n[size/2];
}



static cv::Vec3f MeasureBox(const cv::Mat& depth, const cv::Mat& K, const cv::Mat& D, const cv::Mat& mask,
                            cv::Vec4f table, float heightCut, size_t pointNumberThreshold)
{
    cv::Mat xyz;

    depth2xyz(xyz, depth, K, D);

    cv::Mat masknot;
    cv::bitwise_not(mask, masknot);

    xyz.setTo(cv::Vec3d(0,0,0), masknot);

    std::vector<cv::Vec3f> ps;
    for(size_t i=0;i<xyz.total();i++)
    {
        cv::Vec3d &xyz1=xyz.at<cv::Vec3d>(i);
        if(xyz1[2]>0)
        {
            ps.push_back(cv::Vec3f(xyz1[0],xyz1[1],xyz1[2]));
        }
    }

    cv::Matx33f R;
    cv::Vec3f T;

    PlaneProjection(table, R, T);

    std::vector<cv::Vec2f> xyCut;
    std::vector<float> zCut;

    for(auto &p:ps)
    {
        cv::Vec3f ppr=R*(p-T);
        if(ppr[2]>heightCut)
        {
            xyCut.push_back(cv::Vec2f(ppr[0],ppr[1]));
            zCut.push_back(ppr[2]);
        }
    }

    if(xyCut.size()<pointNumberThreshold)
        return cv::Vec3f(0,0,0);

    std::vector<float> zCopy(zCut.begin(),zCut.end());
    float zMedian=median(zCopy);

    float objectH=zMedian;

    std::vector<cv::Vec2f> xyCut2;

    for(size_t i=0;i<xyCut.size();i++)
    {
        if(fabs(zCut[i]-zMedian)<heightCut)
        {
            xyCut2.push_back(xyCut[i]);
//            objectH+=zCut[i];
        }
    }

//    objectH/=xyCut2.size();

    std::cout<<"found object of "<<xyCut2.size()<<" points\n";


    if(xyCut2.size()<pointNumberThreshold)
        return cv::Vec3f(0,0,0);

    cv::RotatedRect rr=cv::minAreaRect(xyCut2);

    if(rr.size.height>rr.size.width)
        return cv::Vec3f(rr.size.height, rr.size.width, objectH);

    return cv::Vec3f(rr.size.width, rr.size.height, objectH);
}



#endif
